﻿using System;
using System.Collections.Generic;
using System.Linq;
using System.Text;
using Microsoft.Xna.Framework;
using System.Diagnostics;

namespace SpaceFight
{
	class QuaternionHelpers
	{
		public static float Angle(ref Quaternion p_quaternion)
		{
			if(p_quaternion.LengthSquared() > 0)
			{
				p_quaternion.Normalize();
			}
			float retval = 2 * (float)Math.Acos(MathHelper.Clamp(p_quaternion.W, -1, 1));
			Debug.Assert(float.IsNaN(retval) == false);
			return retval;
		}
		public static Vector3 Axis(ref Quaternion p_quaternion)
		{
			if(p_quaternion.LengthSquared() > 0)
			{
				p_quaternion.Normalize();
			}
			float s = (float)Math.Sqrt(Math.Max(0, 1 - p_quaternion.W * p_quaternion.W));
			Debug.Assert(float.IsNaN(s) == false);
			if (s < 0.001) 
			{
				return Vector3.UnitX;
			}
			else
			{
				return new Vector3(p_quaternion.X / s, 
				                   p_quaternion.Y / s,
				                   p_quaternion.Z / s);
			}
		}
		public static Quaternion AngleBetween(ref Vector3 p_v1, ref Vector3 p_v2) 
		{
			float d = Vector3.Dot(p_v1, p_v2);
			float angle = (float)Math.Acos(MathHelper.Clamp(d, -1, 1));
			Vector3 axis = Vector3.Cross(p_v1, p_v2);
			return Quaternion.CreateFromAxisAngle(axis, angle);
		}
		
		/// <summary>
		/// Found this one on the internet. Does weird stuff....
		/// </summary>
		public static Quaternion AngleBetween2(ref Vector3 p_v1, ref Vector3 p_v2) 
		{
			float d = Vector3.Dot(p_v1, p_v2);
			Vector3 axis = Vector3.Cross(p_v1, p_v2);
			float qw = (float)Math.Sqrt(Math.Max(0, p_v1.LengthSquared() * p_v2.LengthSquared() )) + d;
			Quaternion q;
			if (qw < 0.0001)
			{
				// vectors are 180 degrees apart
				q = new Quaternion(0, -p_v1.Z, p_v1.Y, p_v1.X);
			}
			else
			{
				q = new Quaternion(qw, axis.X, axis.Y, axis.Z); 
			}
			if(q.LengthSquared() > 0) q.Normalize();
			return q;
		}

		public static void Clamp(ref Vector3 p_vector, float p_length)
		{
			if (p_vector.Length() > p_length)
			{
				p_vector.Normalize();
				p_vector *= p_length;
			}
		}
		public static void Clamp(ref Quaternion p_quaternion, float p_angle)
		{
			float angle = QuaternionHelpers.Angle(ref p_quaternion);
			if(Math.Abs(angle) > p_angle)
			{
				p_quaternion = Quaternion.CreateFromAxisAngle(
						QuaternionHelpers.Axis(ref p_quaternion), 
						p_angle * (angle/Math.Abs(angle)));
			}
		}
	}
}
